#pragma once
#include <webots/Motor.hpp>
#include <webots/Robot.hpp>
using namespace webots;
#include <string>
#include <cstring>

typedef double spd_t;

enum motor_idx_t
{
    LF_WHEEL_1 = 0,
    RF_WHEEL_2,
    LR_WHEEL_3,
    RR_WHEEL_4,
    TTL_WHEEL_CNT,
};
class wheel_ctl_t
{
private:
    /* data */
    Robot *m_robot;
    Motor *m_motor[TTL_WHEEL_CNT];

public:
    wheel_ctl_t(Robot *robot, const char* motor_name[]);
    ~wheel_ctl_t(){}
    void set_motor_spd(uint16_t index, spd_t speed);
    void set_4motors_spd(spd_t speed,int8_t dir0,int8_t dir1,int8_t dir2,int8_t dir3);
    void go(spd_t speed);
    void left(spd_t speed);
    void right(spd_t speed);
    void back(spd_t speed);
    void stop(void);
};


